function [output,self] = getOutput(self,aircraft,weather,target,time);
%*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*%
%          Copyright (C) 2013 Regents of the University of Colorado.           %
%                             All Rights Reserved.                             %
%                                                                              %
%    This program is free software: you can redistribute it and/or modify      %
%    it under the terms of the GNU General Public License version 2 as         %
%    published by the Free Software Foundation.                                %
%                                                                              %
%    This program is distributed in the hope that it will be useful,           %
%    but WITHOUT ANY WARRANTY; without even the implied warranty of            %
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             %
%    GNU General Public License for more details.                              %
%                                                                              %
%    You should have received a copy of the GNU General Public License         %
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.     %
%                                                                              %
%           Jack Elston                                                        %
%           elstonj@colorado.edu                                               %
%                                                                              %
%*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*%

  %t=floor((time-settings.time.initial)/settings.time.step) +1;
  %ua_x = floor((aircraft.x-settings.grid.x0)/settings.grid.xstep) +1;
  %ua_y = floor((aircraft.y-settings.grid.y0)/settings.grid.ystep) +1;

  vector = aircraft.position;
  x = vector(1);
  y = vector(2);
  z = vector(3);

  vector = aircraft.euler;
  roll = vector(1);
  pitch = vector(2);
  yaw = vector(3);

  switch self.type

    case 'p'
      targetangle=-(atan2(target.y-y,target.x-x)-pi/2);
      if (targetangle < 0)    targetangle=targetangle+2*pi; end
      if (targetangle > 2*pi) targetangle=targetangle-2*pi; end

      diff=(yaw-targetangle);
      if (diff < -pi) diff = 2*pi + diff; end
      if (diff > pi)  diff = diff-2*pi;   end
      
      u2 = -0.1*(diff);
      u1 = aircraft.vmax;

    case 'pid'
      %targetangle=-(atan2(target.y-y,target.x-x)-pi/2);

      %--- feedforward stuff ---%
      vector = weather(time, aircraft.position);
      u = vector(1);
      v = vector(2);

      pos = aircraft.position;
      distance = norm([pos(1) pos(2)]-[target.x target.y]);
      remaining = distance/aircraft.velocity;

      u = u*remaining;
      v = v*remaining;

      targetangle=-(atan2(target.y-v-y,target.x-u-x)-pi/2);
      %--- feedforward stuff ---%

      %targetangle=-(atan2(target.y-y,target.x-x)-pi/2);

      if (targetangle < 0)    targetangle=targetangle+2*pi; end
      if (targetangle > 2*pi) targetangle=targetangle-2*pi; end

      diff=(yaw-targetangle);
      if (diff < -pi) diff = 2*pi + diff; end
      if (diff > pi)  diff = diff-2*pi;   end

      d_diff = self.diff - diff;
      i_diff = self.err + diff;
      self.diff = diff;
      self.err = i_diff;

      u2 = - 2*(diff) - 0.5*(d_diff) - 0*(i_diff);
      u1 = aircraft.vmax;


    case 'c'

      if mod(time,60) == 0 || exist('self.cost_map') == 0
        self.cost_map = generateCostMap(self,weather,aircraft,target,time,z);
        [px,py] = gradient(self.cost_map);

			%self.cost_map
			%node = self.cost_map.findAt([x,y]);
			%neighbor = self.cost_map.findClosest(node)
			%px = (neighbor.V-node.V)/(neighbor.Pos(1)-node.Pos(1));
			%py = (neighbor.V-node.V)/(neighbor.Pos(2)-node.Pos(2));
				
        temp = -px;
        px = -py;
        py = temp;
        px(isnan(px)) = .5;
        px(find(px == Inf)) = .5;
        px(find(px == -Inf)) = -.5;
        py(isnan(py)) = .5;
        py(find(py == Inf)) = .5;
        py(find(py == -Inf)) = -.5;
        self.gradient.px = px;
        self.gradient.py = py;
      end

      px = self.gradient.px;
      py = self.gradient.py;
      frame = weather.frame;
      shift_x = floor((frame.u * (time - weather.t0))/weather.xs);
      shift_y = floor((frame.v * (time - weather.t0))/weather.ys);

			% begin for temporary mesh fix
			%load new_weather;
			%closest_node = new_weather.findAt([x,y]);
			%x = closest_node.Position(1);
			%y = closest_node.Position(2);
			%x = x+1000;
			%y = y+1000;
			% end for temporary mesh fix

      ua_x = floor((x-weather.x0)/weather.xs) + 1 - shift_x;
      ua_y = floor((y-weather.y0)/weather.ys) + 1 - shift_y;

      targetangle=-(atan2(py(ua_x,ua_y),px(ua_x,ua_y))-pi/2);
      if (targetangle < 0)    targetangle=targetangle+2*pi; end
      if (targetangle > 2*pi) targetangle=targetangle-2*pi; end

      diff=(yaw-targetangle);
      if (diff < -pi) diff = 2*pi + diff; end
      if (diff > pi)  diff = diff-2*pi;   end

      u2 = -0.1*(diff);
      u1 = aircraft.vmax;

  end

  if (u1 < aircraft.vmin)     u1=aircraft.vmin;     end
  if (u1 > aircraft.vmax)     u1=aircraft.vmax;     end
  if (u2 > aircraft.turnmax)  u2=aircraft.turnmax;  end
  if (u2 < -aircraft.turnmax) u2=-aircraft.turnmax; end

  output = [u1 u2];
